
#include "accel.h"

//unsigned short   accel_raw[3];     // raw data from accel
unsigned short   accel_raw_zero[3];// zero acceleration code
float            accel_scale[3];   // accel convertion scale to g

// Accel scale variables. Call accel_init() after change
extern float IMU_ADC_ACCELX_SCALE;
extern float IMU_ADC_ACCELY_SCALE;
extern float IMU_ADC_ACCELZ_SCALE;

void accel_update_x(unsigned short accel_raw_x)
{
  accel[0] = IMU_ADC_ACCELX_SIGN ((float)(accel_raw_x - accel_raw_zero[0])) * accel_scale[0];
}

void accel_update_y(unsigned short accel_raw_y)
{
  accel[1] = IMU_ADC_ACCELY_SIGN ((float)(accel_raw_y - accel_raw_zero[1])) * accel_scale[1];
}

void accel_update_z(unsigned short accel_raw_z)
{
  accel[2] = IMU_ADC_ACCELZ_SIGN ((float)(accel_raw_z - accel_raw_zero[2])) * accel_scale[2];
}

void accel_init(void)
{
  // accel_raw_zero initialization
  accel_raw_zero[0] = IMU_ADC_ACCELX_ZERO;
  accel_raw_zero[1] = IMU_ADC_ACCELY_ZERO;
  accel_raw_zero[2] = IMU_ADC_ACCELZ_ZERO;

  // accel_scale initialization
  accel_scale[0] = IMU_ADC_ACCELX_SCALE;
  accel_scale[1] = IMU_ADC_ACCELY_SCALE;
  accel_scale[2] = IMU_ADC_ACCELZ_SCALE;
}
